package weeny.util;

import java.awt.geom.Point2D;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import weeny.util.Gaussian.MyGaussian;
import weeny.util.logs.Debug;

//TODO to clean up
public class AngleDistributer {
	
	private double totalWeight;
	private float[] xy;
	private List<Double> angleList  = null;
	private List<Double> weightList = null;
	private MyGaussian gauss = null;
	private boolean isZero2Pi = true;
	public AngleDistributer(boolean isZero2Pi, double totalWeight){
		this.isZero2Pi = isZero2Pi;
		this.totalWeight = totalWeight;
		if (isZero2Pi) {
			xy = newXYArray(0, (float) Math.PI, gap);
		}
		else{
			xy = newXYArray((float) -Math.PI, (float) Math.PI, gap);
		}
		if (totalWeight<1) {
			angleList = new ArrayList<Double>();
			weightList = new ArrayList<Double>();
		}
		gauss = getGauss(totalWeight);
	}
	/**
	 * We dont know the total weight yet.
	 * So we will need to re-generate the distribution
	 * when total weight is growing big enough
	 */
	public AngleDistributer() {
		xy = zeroPiXYArray();
		totalWeight = 1;
		angleList = new ArrayList<Double>();
		weightList = new ArrayList<Double>();
		gauss = getGauss(totalWeight);
	}
	/**
	 * we know the total weight, so the gaussian function is fixed.
	 * (The bigger total weight, the sharper the gaussian function.)
	 * We dont keep all the angles and weights
	 * @param totalWeight
	 */
	public AngleDistributer(double totalWeight) {
		this.totalWeight = totalWeight;
		xy = zeroPiXYArray();
		gauss = getGauss(this.totalWeight);
	}
	
	private void appending(double direction, double weight){
		if (totalWeight<5) {
			weight = weight * weight;
		}
		if (direction < -Math.PI || direction > Math.PI) {
			throw new IllegalArgumentException("direction is not in the range of -PI to PI");
		}
		if (direction < 0 && isZero2Pi)
			direction += Math.PI;
		double threeSigma = gauss.get3Sigma();
		double minAngle = xy[0];
		double maxAngle = xy[xy.length-2];
		double range = maxAngle-minAngle;
		if (direction < threeSigma+minAngle) {
			//if direction is too small, it affects pi side
			for(int i = xy.length-2; maxAngle-xy[i]+direction<threeSigma; i-=2){
				xy[i+1] += gauss.getValue(direction, xy[i]-range);
			}
			for(int i = 0; xy[i]<direction+threeSigma; i+=2){
				xy[i+1] += gauss.getValue(direction, xy[i])*weight;
			}
		}
		else if (direction > maxAngle-threeSigma){
			//if direction is too big, it affects 0 side
			for(int i = 0; Math.PI-direction+xy[i]<threeSigma; i+=2){
				xy[i+1] += gauss.getValue(direction, xy[i]+range)*weight;
			}
			for(int i = xy.length-2; xy[i]>direction-threeSigma; i-=2){
				xy[i+1] += gauss.getValue(direction, xy[i])*weight;
			}
		}
		else {
			//it is neigher too big nor too small
			int start = (int) ((direction-threeSigma)/gap);
			if (start > 0) 
				start--;
			int end   = (int) ((direction+threeSigma)/gap);
			if (end < 179)
				end ++;
			for(start = 0; start<xy.length; start+=2){
				if (xy[start]>=direction-threeSigma) {
					break;
				}
			}
			for(end = start; end<xy.length; end+=2){
				if (xy[end]>direction+threeSigma) {
					break;
				}
			}
			for(int i=start; i<end; i+=2){
				xy[i+1] +=(float)(gauss.getValue(direction, xy[i])*weight);
			}
//			for(int i = start; i<end; i++){
//				xy[i*2+1] += (float) (gauss.getValue(direction, xy[i*2])*weight);
//			}
		}
	}
	
	/**
	 * add one more angle and weight to the distribution
	 * @param direction - Direction, from -PI to PI.
	 * @param weight - How this direction is weighted
	 */
	public void append(double direction, double weight){
		if (angleList  != null) {
			//angles is cached, then we need to update
			//the curve if total weight is becoming too big
			//it means there are too many directions are added,
			//so we need to make the gaussian curve sharper
			angleList.add(direction);
			weightList.add(weight);
			totalWeight += weight;
			
			if (getGauss(totalWeight) != gauss) {
				gauss = getGauss(totalWeight);
				xy = zeroPiXYArray();
				for(int i = 0; i<angleList.size(); i++){
					appending(angleList.get(i), weightList.get(i));
				}
			}
		}
		appending(direction, weight);
	}
	/**
	 * draw the distribution, from 0 to PI
	 */
	public void dump(){
		double max = maxY();
		double unit = max/45;
//		double unit = max > 45 ? max/45 : 1;
		while(max > 0){
			max -= unit;
			for(int i = 0; i<xy.length/2; i++){
				System.out.print(xy[i*2+1]>max? ((i)%10)+"":" ");
			}
			System.out.println();
		}
		StringBuffer marker = new StringBuffer();
		int i = 0;
		while(marker.length()<xy.length/2){
			if (marker.length()%10 == 0) {
				if (isZero2Pi) {
					marker.append(i);
				}
				else{
					marker.append(Math.abs(i-180));
				}
				i+=10;
			}
			else{
				marker.append(" ");
			}
		}
		System.out.println(marker.toString());
		System.out.println();
	}
	
	private static float gap = (float) (Math.PI / 180);
	private static MyGaussian gauss30 = new MyGaussian(0, (float) (Math.PI*3/2/160*8));         //one sigma = ten degrees
	private static MyGaussian gauss10 = new MyGaussian(0, (float) (Math.PI*3/2/160));           //one sigma = ten degrees
	private static MyGaussian gauss5  = new MyGaussian(0, (float) (Math.PI*3/2/160/4));         //one sigma = five degrees
	private static MyGaussian gauss2  = new MyGaussian(0, (float) (Math.PI*3/2/160/4/4));       //one sigma = two degrees
	private static MyGaussian gauss1  = new MyGaussian(0, (float) (Math.PI*3/2/160/4/4/4));     //one sigma = one degree
	private static MyGaussian gauss05 = new MyGaussian(0, (float) (Math.PI*3/2/160/4/4/4/4));   //one sigma = half degree
//	private static MyGaussian gauss025= new MyGaussian(0, (float) (Math.PI*3/2/160/4/4/4/4/4)); //one sigma = quater degree
	private static MyGaussian getGauss(double edgeCnt){
		if (edgeCnt< 5) {
			return gauss30; 
		}
		if (edgeCnt< 10) {
			return gauss10;
		}
		if (edgeCnt < 30){
			return gauss5;
		}
		if (edgeCnt< 100) {
			return gauss2;
		}
		if (edgeCnt< 400) {
			return gauss1;
		}
		return gauss05;
		//if use gauss025, it becomes just like bin (every degree) counting
	}

	private float[] zeroPiXYArray(){
		return newXYArray(0, (float)Math.PI, gap);
	}
	private float[] newXYArray(float minValue, float maxValue, float gap){
		assert minValue < maxValue;
		int size = (int) ((maxValue - minValue) / gap) + 2;
		float[] ret = new float[size * 2];
		for (int i = 0; i < size -1; i++){
			ret[i*2]   = i*gap+minValue;
			ret[i*2+1] = 0f;
		}
		ret[size*2-2] = maxValue;
		ret[size*2-1] = 0f;
		return ret;
	}

	//get the very peak direction form xy array
	public double peakDirection() {
		if (maxY() < 0.01f) {
			System.out.println(Arrays.toString(xy));
			Debug.msg("all y is smaller than 0.01");
		}
		double maxSoFar = Double.MIN_VALUE;
		double peakDirection = 0;
		for (int i = 0; i< xy.length; i+=2) {
			if (xy[i+1] > maxSoFar) {
				peakDirection = xy[i];
				maxSoFar = xy[i+1];
			}
		}
		return peakDirection>Math.PI? Math.PI : peakDirection;
	}
	// get the max value of y fields
	public float maxY() {
		float maxY = Float.MIN_VALUE;
		for (int i = 1; i < xy.length; i += 2) {
			if (xy[i] > maxY) {
				maxY = xy[i];
			}
		}
		return maxY;
	}

//	// get the primary direction form xy array
//	public double primaryDirection() {
//		if (maxY() < 0.01f) {
//			System.out.println(Arrays.toString(xy));
//			Debug.DEBUG("all y is smaller than 0.01");
//		}
//		List<Point2D> peaks = peakPoints(xy);
//		// sort the points, from big to small
//		Collections.sort(peaks, new Comparator<Point2D>() {
//			public int compare(Point2D o1, Point2D o2) {
//				if (o1.getY() > o2.getY()) {
//					return -1;
//				} else if (o1.getY() < o2.getY()) {
//					return 1;
//				} else
//					return 0;
//			}
//		});
//		assert peaks.size() != 0 : "should have at least one point here";
//		double result = peaks.get(0).getX();
//		return result > Math.PI? Math.PI : result;
//	}

	
	//chech if the y value at index of xy is locally max
	private static boolean isLocalMax(float[] xy, int index){
		assert index % 2 == 1;
		if (index == 1) {
			return xy[index] >= xy[index + 2] && xy[index] > xy[xy.length-1];
		}
		else if (index == xy.length - 1) {
			return xy[index] >= xy[1] && xy[index] > xy[index - 2];
		}
		else {
			return xy[index] >= xy[index+2] && xy[index] > xy[index-2];
		}
	}

	public List<Point2D> peakPoints(){
		ArrayList<Point2D> result = new ArrayList<Point2D>();
		for (int i = 0; i < xy.length; i+=2) {
			if (isLocalMax(xy, i+1)) {
				result.add(new Point2D.Float(xy[i], xy[i+1]));
			}
		}
		return result;
	}
}
